#include "AlgoEvaluatorModule4AcousticStealth.h"
#include <cmath> // For pow, log10

class algo::AlgoEvaluatorModule4AcousticStealthPrivate 
{
public:
};

algo::AlgoEvaluatorModule4AcousticStealth::AlgoEvaluatorModule4AcousticStealth()
	: d(new AlgoEvaluatorModule4AcousticStealthPrivate)
{
}

algo::AlgoEvaluatorModule4AcousticStealth::~AlgoEvaluatorModule4AcousticStealth()
{
	delete d;
}

std::vector<algobase::EvaluateEnvirDataType> algo::AlgoEvaluatorModule4AcousticStealth::getNeedType()
{
	return std::vector<algobase::EvaluateEnvirDataType>();
}

// This function calculates the radiated noise level (in dB) based on speed.
// The model and parameters are illustrative and should be replaced with real-world data.
// SL(v) = SL_base + 60 * log10(v / v_ref)
// This is a common model where noise level increases by 18dB per doubling of speed.
static double calculate_self_noise_db(double speed_mps)
{
	// Assume speed is in meters per second.
	if (speed_mps < 0.1) // Below a certain speed, assume a constant base noise level.
	{
		speed_mps = 0.1;
	}

	const double SL_base = 70.0; // Base noise level in dB at reference speed.
	const double v_ref = 1.0;    // Reference speed in m/s (e.g., 1 m/s).
	
	// Using a simplified model: Noise Power is proportional to v^6, which means dB is proportional to 60*log10(v)
	return SL_base + 60.0 * std::log10(speed_mps / v_ref);
}


int algo::AlgoEvaluatorModule4AcousticStealth::exec(const algobase::Path& path, double& result, algobase::EvaluateModuleCoords& indexCoords)
{
	if (path.empty())
	{
		result = 0.0; // No movement, no noise. Or a base level noise.
		indexCoords.clear();
		return 0;
	}

	double total_linear_self_noise = 0.0;
	indexCoords.resize(path.size());

	for (size_t i = 0; i < path.size(); ++i)
	{
		indexCoords[i].Assign(path[i]);

		double speed_mps = path[i].mSpeed;
		double point_noise_db = calculate_self_noise_db(speed_mps);
		
		indexCoords[i].mRawValue = point_noise_db; // Store dB value for this point.
		indexCoords[i].mResult = calcScore(point_noise_db);

		// Convert dB to linear scale for averaging
		total_linear_self_noise += std::pow(10.0, point_noise_db / 10.0);
	}

	// Calculate the average linear self-noise
	double avg_linear_noise = total_linear_self_noise / path.size();

	// Convert the average back to dB to get the final raw value
	if (avg_linear_noise > 0) {
		result = calcScore(10.0 * std::log10(avg_linear_noise));
	} else {
		result = calcScore(0.0); // Should not happen if base noise is handled.
	}

	return 0;
}

double algo::AlgoEvaluatorModule4AcousticStealth::calcScore(double rawValue) const
{
	return costSigmoid(rawValue, 80.0, 5.0) * getWeight();
}

double algo::AlgoEvaluatorModule4AcousticStealth::calcRaw(const std::vector<double>& rawValues) const
{
	if (rawValues.empty()) {
		return 0.0;
	}

	double total_linear_self_noise = 0.0;
	for (size_t i = 0; i < rawValues.size(); ++i) {
		total_linear_self_noise += std::pow(10.0, rawValues[i] / 10.0);
	}

	// Calculate the average linear self-noise
	double avg_linear_noise = total_linear_self_noise / rawValues.size();
	return 10.0 * std::log10(avg_linear_noise);
}
